/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file XMLtoData.cxx
  \brief class implemenation XML parser

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include "dhl.h"
#include "dhl-struct.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
  extern bool verbose;
/*<-End Global Variables-->*/

DHL::DHL(String name) : ThreadedPipe(name)
{
  xb_init = false;
  my_addr = 0;
  seq_num = 0;
  last_addr_req.stamp();
  first_time_subscriber = true;
}

void DHL::setAddress(uint16_t xb_addr)
{
  my_addr = xb_addr;
  Client * me = shmPtr->clients.getClient(0);
  in_addr address;
  address.s_addr = inet_addr("6.1.0.0");
  address.s_addr |= ((uint32_t)xb_addr) << 24;
  address.s_addr |= ((uint32_t)(xb_addr & 0xFF00)) << 8;
  shmPtr->clients.addAddr(me,address);
}

void DHL::translate() {
  // This function assumes we've gotten a complete XBee packet on the Queue 
  // since we've gotten a *valid* checksum, i'm assuming its not fragmented
  int ZigbeeHeaderSize = XB_RX_PAYLOAD;
  char buf[1024] = {'\0'};
  struct xb_rx_hdr* xb_hdr;
  struct dhl_hdr* mdhl_hdr;
  int i;

  // Pop the Queue until we're at the start of the valid XBee packet
  while (xbQueue.size() && ((buf[0] = xbQueue.popByte()) != XB_DELIMETER));

  if (xbQueue.size() <= 0) {
    //couldn't find starting delimeter
    return;
  }

  // pop the zigbee header
  for (i = 1; i < ZigbeeHeaderSize; i++)
    buf[i] = xbQueue.popByte();

  xb_hdr = (struct xb_rx_hdr*)buf;
  xb_hdr->size = cupic_to_xb(xb_hdr->size);
  //cout << "Size is: " << xb_hdr->size << endl;
  //xb_hdr->dest = cupic_to_xb(xb_hdr->dest);

  // pop the DHL header
  // XB delimeter + size adds 3 bytes (they're not included in the size)
  while (i < xb_hdr->size + 3)
    buf[i++] = xbQueue.popByte();

  // dump the entire packet
  //for (int j = 0; j < i+SPACING; j++)
    //printf("%.02hhX ", buf[j]);
  //printf("\n");

  // pop the CSUM (which xb's verified for us already)
  xbQueue.popByte();

  mdhl_hdr = (struct dhl_hdr*)(&buf[XB_RX_PAYLOAD]);
  mdhl_hdr->size = cupic_to_xb(mdhl_hdr->size);

  // MAV Addresses are 16 bits
  // set the source address to OUR source address
  fifo_msg.src.s_addr = inet_addr("6.1.0.0");
  fifo_msg.src.s_addr |= ((uint32_t)my_addr) << 24;
  fifo_msg.src.s_addr |= ((uint32_t)(my_addr & 0xFF00)) << 8;

  switch (mdhl_hdr->type) {
    case CMD_CAPABILITY: {
      DHL_Capabilities_t capability;
      SystemCharacter_t system;
      Client *ds, *me;
      in_addr addr;

      addr.s_addr = inet_addr("6.1.0.0");
      addr.s_addr |= ((uint32_t)xb_hdr->dest) << 16;

      String name(inet_ntoa(addr));

      // copy the data so we don't over-write it later
      memcpy(&capability, &buf[DHL_PAYLOAD], sizeof(DHL_Capabilities_t));

      // get the daughtership and the dhl clients
      ds = shmPtr->clients.getClient(addr);
      me = shmPtr->clients.getClient(0);

      // if the ds client doesn't exist yet create it.
      // the DHL will maintain this client for each ds
      if (ds == NULL) {
        shmPtr->clients.add(name);
        ds = shmPtr->clients.getClient(name);

        shmPtr->clients.addAddr(ds, fifo_msg.src);
        shmPtr->clients.addAddr(ds, addr);
      }

      // translate the DHL Capabilities into a system capabilities
      system.id = capability.sub_system_id;
      system.commands = capability.commands;
      system.streams = capability.streams;

      // FIXME: set attributes parameters to thier actual values
      system.range = 0;
      system.fov = 0;
      system.direction = 0;

      // add capabilities to the client (if we havent already)
      ds->capabilities.add(&system);

      // fill out the fifo_msg ....
      ds->capabilities.getCapabilities((uint8_t*)fifo_msg.data);
      fifo_msg.size = CMD_SIZE(fifo_msg.data);

      // ... and forward it to the other MHL nodes
      for (int i = 0; me->address[i].s_addr != INADDR_ANY; i++) {
        fifo_msg.dest.s_addr = me->bcast_addr[i].s_addr;
        fifo_msg.port = 1000;

        writeFifo(INBOARD_TX);
      }
    } break;

    // IF necessary insert translation chunks here
    case CMD_CUSTODY:
      cout << "CMD_CUSTODY" << endl;
      break;

    case CMD_DEBUG:
      cout << "CMD_CUPIC_DEBUG" << endl;
      break;

    case CMD_HOUSE_KEEPING:
      cout << "CMD_HOUSE_KEEPING" << endl;
      break;

    case CMD_LEDS:
      cout << "CMD_LEDS" << endl;
      break;

    default:
      //cout << "recieved unknown packet type: " << (int)mdhl_hdr->type << ", doing nothing" << endl;
      break;
  }
}

void DHL::update() 
{
  uint16_t MavNum;
  int val, maxFD;
  fd_set readFDs, writeFDs;                   // fd sets
  struct timeval timeout;

  if(!xb_init) {
    currentStamp.stamp();
    if((currentStamp - last_addr_req ) > (ADDR_REQ_PERIOD * SEC2MICRO) ) {
      xbQueue.requestAddress((uint8_t *)fifo_msg.data);
      fifo_msg.size = 8;
      writeFifo(OUTBOARD_TX);
      last_addr_req.stamp();
    }
  }

  FD_ZERO(&readFDs);                  // zero read fd set
  FD_ZERO(&writeFDs);                  // zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);      // add fifo
  FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);      // add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);      // add fifo
  FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);    // add fifo

  fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("DHL::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {
    // reading from XB
    if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
    {
      int n = 0, zigbee_size= 0;
      //uint8_t byte[1]={0};

      n=readFifo(OUTBOARD_RX);

      if(n>0) {
        xbQueue.push((uint8_t*)fifo_msg.data,n);
        
        while( !xbQueue.APIneedMoreBytes() ) {
          if(xbQueue.APIisValid()){
            //cout << "  good API_Csum\n";
            MavNum= xbQueue.getMavNum();
            //cout << MavNum << endl; 

            zigbee_size= xbQueue.size();

            // Getting our XBee address
            if(!xb_init) {
              uint16_t length; 
              uint16_t address;

              xbQueue.popByte();
              length = (uint16_t)(xbQueue.popByte() << 8);
              length |= (uint16_t)(xbQueue.popByte());
              
              if(length == 7)
                if(xbQueue.popByte() == 0x88)
                  if(xbQueue.popByte() != 0x0)
                    if(xbQueue.popByte() == 'M')
                      if(xbQueue.popByte() == 'Y')
                        if(xbQueue.popByte() == 0) {
                          address = (uint16_t)(xbQueue.popByte() << 8);
                          address |= (uint16_t)(xbQueue.popByte());
                          setAddress(address);
                          xb_init = true;
                        }
            }

            // Getting data from some XBee mote
            else {
              translate();
            }
          }//end of: if(xbQueue.APIisValid())
          
          else{
            if(verbose)
              cout << "bad API_Csum" << endl;
            xbQueue.popByte();
          }
        }//end of: while( !xbQueue.APIneedMoreBytes() 
      }//end of: if(n>0)
    }//end of: if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))

    if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
      int n = 0;
      n=readFifo(INBOARD_RX);

      // send data outward
      Command cmd(fifo_msg.data);
      if ( !cmd.isValid() ) {
        cout << getName() << "::update() - error, invalid dommand" << endl << cmd;
        return;
      }

      //Client *a_client = shmPtr->clients.getClient(fifo_msg.src);
      //uint8_t *dataPtr = CMD_DATAPTR(fifo_msg.data);

      switch (cmd.cmd()) {
        case CMD_STREAM_REQ: {
          // this should never happen, StreamRequests are handled by the 
          // data distribution block
          break;
        }
        /* Note that translation from the MHL->DHL goes on here */
        default:
          cout << "got cmd: " << cmd.cmd() << endl;
      }
    }

    // send updates to various nodes...
    if (FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
      // see whether anybody's request data:

      Client * a_client;
      int i = 0;

      // subscribe, IF nobody has yet (or only person has so far)
      while ((a_client = shmPtr->clients.getSubscriptionsTo(CMD_DEBUG,i++)) != NULL) {
        if (first_time_subscriber) {
          // create the subscription packet
          first_time_subscriber = false;

          fifo_msg.size = build_stream_request(
            fifo_msg.data, 
            128, 
            my_addr, 
            (cu_uint_t)0x6002,
            seq_num++, 
            CMD_DEBUG, 
            true
          );

          //for (int i = 0; i < size; i++)
            //printf("%.02hhX ", fifo_msg.data[i]);
          //printf("\n");

          writeFifo(OUTBOARD_TX);
        }
      }

      // unsubscribe (if there are no more subscribers
      if ((i == 1) && (first_time_subscriber == false)) {
        first_time_subscriber = true;

        fifo_msg.size = build_stream_request(
          fifo_msg.data, 
          128, 
          my_addr, 
          0x6002,
          seq_num++, 
          CMD_DEBUG, 
          false
        );

        writeFifo(OUTBOARD_TX);
      }
    }
  }
}

void DHL::sendLEDCommand(char cmd) {
  int size;

  cu_uchar_t buf[128]; // MAX XB Out size
  
  // FIXME: for debugging this is ok, but dont broadcast the packet
  size = build_led_packet(buf, 128, my_addr, 0xFFFF, seq_num++, cmd);

  //for (int i = 0; i < size; i++)
    //printf("%.02hhX ", buf[i]);
  //printf("\n");

  memcpy(fifo_msg.data, buf, size);

  fifo_msg.size = size;
  writeFifo(OUTBOARD_TX);
}
